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Abstract — This  paper  discusses  an  approach  to  using  the 
Cramer  Rao  Lower  Bound  (CRLB)  as  a  trajectory  design  tool 
for  autonomous  underwater  vehicle  (AUV)  visual  navigation.  We 
begin  with  a  discussion  of  Fisher  Information  as  a  measure  of  the 
lower  bound  of  uncertainty  in  a  simultaneous  localization  and 
mapping  (SLAM)  pose-graph.  Treating  the  AUV  trajectory  as 
an  non-random  parameter,  the  Fisher  information  is  calculated 
from  the  CRLB  derivation,  and  depends  only  upon  path  geometry 
and  sensor  noise.  The  effect  of  the  trajectory  design  parameters 
are  evaluated  by  calculating  the  CRLB  with  different  parameter 
sets.  Next,  optimal  survey  parameters  are  selected  to  improve 
the  overall  coverage  rate  while  maintaining  an  acceptable  level 
of  localization  precision  for  a  fixed  number  of  pose  samples. 
The  utility  of  the  CRLB  as  a  design  tool  in  pre-planning  an 
AUV  survey  is  demonstrated  using  a  synthetic  data  set  for  a 
boustrophedon  survey.  In  this  demonstration,  we  compare  the 
CRLB  of  the  improved  survey  plan  with  that  of  an  actual 
previous  hull-inspection  survey  plan  of  the  US S  Saratoga.  Survey 
optimality  is  evaluated  by  measuring  the  overall  coverage  area 
and  CRLB  localization  precision  for  a  fixed  number  of  nodes  in 
the  graph.  We  also  examine  how  to  exploit  prior  knowledge  of 
environmental  feature  distribution  in  the  survey  plan. 

I.  Introduction 

Recent  developments  in  autonomous  navigation  have  ex¬ 
panded  autonomous  underwater  vehicle  (AUV)  applications 
to  such  areas  as  automated  ship  hull  inspection  [1],  [2], 
autonomous  underwater  cave  exploration  [3],  mapping  of  ar¬ 
chaeological  sites  [4],  [5],  underwater  exploration  by  tracking 
chemical  plumes  [6],  and  surveys  of  the  underwater  envi¬ 
ronment  [7]— [9] .  For  many  AUV  missions,  a  typical  survey 
criterion  is  to  cover  the  largest  survey  area  possible  while 
satisfying  a  user-defined  level  of  acceptable  navigation  per¬ 
formance.  However,  a  physical  limiting  constraint  in  AUV 
mission  duration  is  that  they  are  governed  by  the  amount  of 
battery  power  that  can  be  carried.  Therefore,  due  to  this  power 
limitation,  an  optimal  path  and  an  efficient  survey  plan  become 
a  key  aspect  of  the  mission  planning. 

There  are  numerous  examples  in  the  literature  for  online 
path  planning  for  AUVs;  for  example,  obstacle  avoidance 
using  sonar  sensing  [10]  and  optimized  trajectories  for  energy 
efficiency  in  the  presence  of  water  currents  [11],  [12].  What 
distinguishes  our  work  is  that  what  we  are  proposing  is  not 
an  online  method  but  rather  a  planning  method  to  be  used 
as  a  design  tool  for  operator  mission  planning — allowing  the 
AUV  operator  to  make  an  informed  trade  off  in  coverage  rate 


Fig.  1.  A  boustrophedon  survey  trajectory  and  its  design  parameters.  The 
along-trackline-space,  d,  and  cross-trackline-space,  h,  determine  the  pose- 
graph  node  sampling,  denoted  as  an  x,  in  the  graph.  The  nodes,  for  example, 
could  represent  trajectory  samples  when  optical  imagery  or  sonar  data  along 
the  hull  are  collected. 

and  localization  precision  for  the  typical  boustrophedon  survey 
pattern.  In  our  application,  these  relative-pose  constraints  are 
derived  from  a  visual  simultaneous  localization  and  mapping 
(SLAM)  system  [13];  however,  they  could  as  equally  come 
from  Iterated  Closest  Point  (ICP)  constraints  derived  from 
sonar  [14]  or  laser  [15]  scan  matching.  In  this  paper,  we 
define  a  performance  metric  based  upon  information  gain  as 
a  measure  of  the  survey’s  uncertainty  level,  and  subsequently 
use  this  metric  to  provide  optimal  trajectory  parameters  in  the 
design  phase. 

A.  Related  Work 

A  recent  focus  area  in  the  robotics  community  has  been 
to  use  information  gain  as  a  measure  of  uncertainty  in  order 
to  improve  the  navigation  and  mapping  accuracy  of  mobile 
platforms.  Typically,  information  gain  refers  to  either  Fisher 
Information  or  Mutual  Information,  where  the  Fisher  infor¬ 
mation  matrix  is  closely  related  to  the  Cramer  Rao  Lower 
Bound  (CRLB)  [16],  and  Mutual  information  is  defined  from 
entropy  [17].  In  [18],  Bingham  introduced  an  approach  for 
AUV  navigation  design  using  the  CRLB  in  the  survey  design 
phase.  In  [19],  Dogancay  used  the  CRLB  to  investigate  the 
design  of  optimal  AOA  (Angle  of  Arrival)  for  Unmanned 
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Aerial  Vehicle  (UAV)  tracking.  In  [20],  Sim  used  the  trace 
of  the  Fisher  information  matrix  to  determine  the  information 
gain  in  pose-graph  SLAM.  This  direction  of  research  was 
further  developed  in  [21],  where  Vidal-Calleja’s  developed 
two  different  strategies  related  to  information  gain  measures 
together  with  Mutual  information  gain. 

B.  Motivation 

In  our  previous  work,  we  implemented  a  vision-based 
SLAM  algorithm,  using  1 ,000  images  to  map  a  portion  of  the 
hull  of  the  USS  Saratoga  aircraft  carrier  (Fig.  2  and  Fig.  9). 
This  work  was  done  under  the  auspices  of  developing  an 
AUV  application  for  autonomous  ship  hull  inspection  [13]. 
The  survey  was  designed  to  follow  a  boustrophedon  trajectory 
along  the  hull  using  a  pre-determined  along-trackline-space, 
d,  and  cross-trackline-space,  h  (Fig.  1).  The  survey  track 
line  distances  were  determined  ahead  of  time  by  the  AUV 
operators  based  upon  the  desired  percentage  overlap  in  the 
imagery,  which  was  naively  calculated  from  the  expected 
vehicle  position  without  taking  into  account  the  pose  sam¬ 
pling  effect  on  vehicle  trajectory  uncertainty.  The  hull-looking 
camera  was  set  to  operate  at  a  fixed  frame  rate  throughout 
the  survey  without  any  consideration  to  energy  expenditure 
with  each  strobed  image  capture.  In  retrospect,  the  previous 
survey  could  have  been  more  efficient  with  consideration  to  the 
amount  of  covered  survey  area,  energy  efficiency,  and  desired 
precision  of  localization  performance  had  these  criteria  been 
simultaneously  considered  in  the  survey  design  phase. 

C.  Outline 

Toward  a  solution  to  the  optimal  trajectory  design  problem, 
this  paper  examines  using  the  CRLB  as  a  metric  for  evaluating 
the  effect  of  cross-trackline-spacing,  d,  and  along-trackline- 
spacing,  h,  on  the  resulting  boustrophedon  pose-graph  uncer¬ 
tainty.  In  §11  we  begin  with  a  derivation  of  the  CRLB  assuming 
a  standard  sensor  combination  for  AUVs,  and  introduce  a 
metric  to  measure  the  graph  uncertainty  level.  We  illustrate 
the  utility  of  this  metric  for  a  simple  example  case  and  explain 
its  interpretation  as  a  planning  tool.  In  §111  we  then  apply  this 
design  tool  to  the  ship  hull  survey  motivation  example,  and  a 
comparison  between  the  actual  survey  and  the  CRLB  improved 
one  is  made.  Finally,  in  §IV  we  offer  some  concluding  remarks 
regarding  what  directions  future  work  should  take  to  improve 
this  design  tool. 

II.  Cramer  R ao  Lower  Bound 

We  assume  the  AUV  moves  in  a  planar  boustrophedon 
trajectory  with  a  camera,  a  compass,  and  an  odometry  sensor 
(e.g.,  integrated  Doppler  velocity  log  (DVL)).  We  use  notation 
of  Xi  =  [xi,yi]T  for  the  position  of  the  vehicle  along  the 
trajectory  at  sample  time  i,  and  6i  for  the  heading  angle  of 
the  robot,  where  X{  =  [x^,6yT  indicates  the  state  of  the 
vehicle.  We  use  Z  to  indicate  the  stacked  vector  of  all  sensor 
measurements  assembled  from  camera  Zc,  odometry  ZG,  and 
compass  Z^  measurements,  and  X  to  indicate  the  stacked 
vector  of  all  trajectory  samples  X{. 


Fig.  2.  Competing  objectives  in  AUV  mission  planning.  The  desired  level 
of  position  uncertainty,  energy  efficiency,  and  available  feature  content  in  the 
environment  all  impact  the  optimal  SLAM  trajectory  design. 


We  use  the  CRLB  (see  [16]  for  standard  reference) 

E[[X(Z)  -  X0][X(Z)  -  X0]T]  >  J-1  (1) 


as  a  measure  of  the  conservative  uncertainty  bound  for  the 
pose-graph  where  J  in  this  equation  is  the  Fisher  information 
matrix 


J  =  E[[VxlnA(X)][VxlnA(X)]T] 


(2) 


for  the  measurement  likelihood  A(X)  —  p(Z|X). 

When  we  assume  that  all  sensor  measurements  are  indepen¬ 
dent,  then  the  likelihood  factorizes  to  a  simple  product  whom 
after  which  taking  its  logarithm  becomes  a  summation  of  all 
sensor  log-likelihoods 


A(X)  =  p(Z|X) 

=  p(Zc|X)p(Z0|X)p(Zh|X)  (3) 

lnA(X)  =  lnp(Zc|X)  +lnp(Z0|X)  +lnp(Zf,|X). 


We  model  the  sensor  measurements  as  corrupted  by  Gaussian 
noise  with  zero  mean  and  covariances  Ec.i?  £0.,  and 
respectively: 

Camera:  ZCy  =  g {XuXj)  +  WCi. ,  WCij  ~  V(0,  ECy), 
Odometry:  Z0i  =  (x,  -  x*_i)  +  WQi,  WQi  ~  J\f( 0,  £0i), 
Compass:  Zhi  =  <9*  +  Whi,  Whi  ~  Af( 0,  a\. ), 


where  we  assume  a  standard  deviation  of  ±1.2  cm/s  in  DVL 
velocity  in  computing  the  integrated  odometry  measurement 
[22]  and  a  fixed  ±1°  for  heading  uncertainty  (i.e.,  cr^.  =  a  h  = 
±1°). 

A.  Modeling  of  the  Camera  Measurement 

For  the  camera  measurement  we  use  a  reduced  two  degree 
of  freedom  (DOF)  camera  measurement  model  g (Xi,Xj) 
for  the  planar  survey,  which  measures  the  azimuth,  and 
relative  orientation,  faj,  between  nodes  i  and  j,  which  models 


(a)  Pairwise  overlap  model  of  camera  measurement 


camera  uncertainty  w.r.t.  the  image  overlap 


Overlap  ratio  [%] 


(b)  Standard  deviation  of  the  camera  measurement  noise  as  a 
function  of  the  overlap  ratio 

Fig.  3.  Camera  measurement  noise  is  modeled  as  a  function  of  the  image 
overlap  ratio  from  the  pairwise  camera  measurement. 

the  camera  as  a  bearing  sensor  making  both  sequential  and 
non- sequential  links  in  the  graph. 


aij 

atan2(yi  -  yi:Xj  -  Xi) 

fiij  _ 

The  camera  sensor  measurement  noise  is  modeled  so  as 
to  be  dependent  on  the  distance  between  two  nodes  where 
the  camera  measurement  uncertainty  ECij.  is  modeled  as  a 
function  of  the  overlap  ratio.  The  size  of  the  overlap  region 
is  commensurate  with  the  proximity  of  node  i  and  j\  the 
closer  nodes  i  and  j,  the  larger  the  overlap  region,  resulting 
in  less  uncertainty  in  the  camera  measurement.  As  can  been 
seen  from  Fig  3(a),  with  a  fixed  camera  field  of  view  (FOV) 
and  the  camera  scene  depth  27,  the  overlap  ratio  becomes  a 
function  of  the  distance  of  two  nodes  only.  This  fixed  scene 
depth  assumption  is  for  the  survey  of  the  bottom  or  the  wall 
following.  For  simplicity,  we  assume  the  footprint  of  the  cam¬ 
era  measurement  to  be  a  circle  as  seen  in  Fig  3(a),  and  define 
camera  measurement  threshold  C thresh  =  227  tan(FOV/2) 

and  the  overlap  ratio  as  7  =  - 211 .  Only  when  the  dis- 

tance  77 j  is  smaller  than  C thresh,  is  the  camera  measurement 
meaningful  and  the  information  gain  is  proportional  to  the 
distance  77 j.  The  function  that  defines  the  uncertainty  of  the 
camera  measurement 

cr  =  ±1°75  (4) 

is  shown  in  Fig.  3(b).  When  the  overlap  ratio  falls  below  10%, 
the  uncertainty  increases  significantly,  and  for  greater  than 
50%  it  remains  relatively  flat.  We  assume  the  camera  FOV 
to  be  40°,  which  gives  the  Cthresh  to  be  0.7279  m  at  the 
scene  depth  of  1  m,  for  example. 


B.  Fisher  Information  Matrix 

As  previously  shown  in  (2),  the  Fisher  Information  ma¬ 
trix,  J,  is  simply  the  expectation  of  the  squared  gra¬ 
dient  of  the  log-likelihood  evaluated  at  the  true  pa¬ 
rameters  Xo.  Using  the  assumption  of  sensor  measure¬ 
ment  independence,  the  expectation  of  the  gradient  from 
two  different  sensor  measurement  likelihoods  is  zero  (i.e., 
E[[Vxlnp(Zm|X)][Vxlnp(Zn|X)]T]  =  0  when  m  ^  n). 
Therefore,  J  reduces  to  the  three  simple  sensor  terms  below 

J  =  E[[VxMZc|X)][Vxlnp(Zc|X)]T] 

+  E[[VX  lnp(Z0|X)][Vx  lnp(Z0|X)]T] 

+  E[[Vxlnp(Z^|X)][Vxlnp(Z^|X)]T]. 

By  defining  information  matrices  Lc,  L0,  and  L^,  to  cor¬ 
respond  to  the  three  terms  above,  respectively,  the  Fisher 
information  J  can  be  written  as  the  summation  of  the  three 
information  matrices  as  contributed  from  each  sensor 

J  =  L0  +  Lh  +  Lc. 

The  detailed  general  structure  for  the  three  information  matri¬ 
ces  is  given  in  equations  (6)  and  (7). 

Unlike  the  odometry  and  heading  sensors,  the  camera  in¬ 
formation  matrix,  Lc,  is  a  function  of  geometry  and  can  be 
written  in  terms  of  the  camera  observation  model  gradient  as 

dgjX^Xj)  dg(XllXJ)T 
13  dXi  Ci i  dXj  '  (  } 

While  the  information  matrix  of  the  odometry  and  heading 
sensors  depend  only  on  the  sensor  uncertainty  and  integration 
interval,  (5)  makes  it  clear  that  the  camera  information  is  a 
function  of  the  sensor  uncertainty  and  the  trajectory  geometry. 
In  particular,  Lc  will  exhibit  nonzero  elements  in  the  off 
diagonal  in  the  Fisher  information  matrix  as  the  camera 
measurement  can  result  from  non-sequential  nodes  in  the 
graph. 

C.  CRLB  Derivation  Example 

In  this  section,  we  examine  a  simple  pose-graph  consisting 
of  three  nodes  as  depicted  in  Fig.  4,  and  present  a  detailed 
calculation  of  the  three  sensor  information  matrices.  In  this 
example,  two  sequential  nodes,  (1,2),  and  (2,3),  are  con¬ 
strained  by  the  odometry  sensor  measurement.  On  each  node, 
we  have  compass  information,  but  we  only  have  the  camera 
measurement  between  nodes  2  and  3,  which  is  depicted  by  the 
green  line  in  Fig.  4.  As  we  have  three  elements  x,  y,  and  6 
in  our  state  representation,  the  three  information  matrices  will 
each  be  9  x  9  matrices. 

Under  this  scenario,  Lc  is  calculated  as 
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Fig.  4.  A  simple  example  of  a  three  node  graph  to  illustrate  the  derivation 
of  the  information  matrices  in  the  CRLB  calculation. 
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Summing  all  terms,  the  Fisher  information  matrix  is  calcu¬ 
lated  to  be  J  =  Lh  +  L0  +  Lc,  which  is  singular  because 
it  contains  only  relative-pose  information  (i.e.  camera  and 
odometry  measurements  are  unaffected  by  a  linear  translation). 
Therefore,  we  fix  the  location  of  the  first  node  in  the  graph 
by  setting  its  information  to  be  infinity  (in  practice  a  very 
large  number  like  106),  so  that  the  other  node  uncertainties 
are  relative  to  the  first  node  position. 

Taking  the  inverse  of  the  resulting  J  matrix  yields  the  9x9 


covariance  matrix  S 

'S11 

S12 
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where  indicates  the  covariance  of  the  ith  node  and  the 
ith ,  jth  cross-covariance. 

D.  Effect  of  the  Design  Parameters 

We  examine  the  CRLB  by  evaluating  the  inverse  of  the 
Fisher  Information  matrix,  which  gives  the  covariance  matrix, 
S,  of  the  overall  graph.  The  determinant  of  each  covariance 
matrix  sub-block,  S^,  reveals  the  uncertainty  of  each  node  in 
the  graph.  We  evaluate  the  maximum  xy  standard  deviation 
for  all  nodes  in  the  graph  (max^S^..  |)  by  looking  at  the 
Sxy  sub-block  for  each  node  and  use  this  as  a  metric  for  the 
level  of  xy  uncertainty  in  the  entire  trajectory.  Figures  5  and  6 
depict  this  survey  metric  for  a  more  extended  case  considering 
a  40  node  trajectory. 

With  respect  to  the  odometry  sensor  uncertainty  on  the  ith 
node,  for  example,  £0.  will  increase  as  the  along-trackline- 
space,  d,  increases  because  it  accumulates  DVL  uncertainty 
over  longer  distances.  On  the  other  hand,  the  reduction  of  the 
cross-trackline-space,  h,  will  enhance  the  overall  pose-graph 
link  structure  as  the  camera  can  make  a  larger  number  of  non¬ 
sequential  links  to  other  tracklines.  In  terms  of  the  camera 
measurements,  small  d  and  h  values  enable  a  higher  density 
of  camera  measurements,  thereby  reducing  the  uncertainty; 
however,  this  also  reduces  the  size  of  the  survey  coverage 
area  as  depicted  in  plans  A,  and  D  of  Fig.  5(a).  Therefore  we 
examine  the  relation  of  the  uncertainty  and  the  survey  coverage 
with  respect  to  the  design  parameters  to  achieve  an  optimal 
balance  given  these  competing  considerations. 

The  two  graphs  in  Fig  5(b)  show  the  relation  of  the  camera 
measurement  with  the  two  independent  design  parameters 
d  and  h.  As  can  be  seen  in  Fig.  5(b),  the  maximum  pose 


d=0.1  d=1.2  d=0.7  and  h=0.5  h=0.1  h=1.0 


(b)  Evaludated  uncertainty  w.r.t.  d  and  h 


Fig.  5.  Change  of  maximum  uncertainty  with  respect  to  the  altitude  normalized  along-trackline-space,  d,  and  the  cross-trackline-space,  h.  Example  trajectory 
design  paths  are  depicted  in  (a). 


Uncertainty  graph  and  coverage  area  contour 


Fig.  6.  The  contour  plot  of  the  maximum  uncertainty  and  the  coverage  area. 
The  letters  from  A  to  E  indicate  each  sample  plan  in  Fig.  5.  The  curved 
lines  with  numbers  indicate  the  altitude  normalized  coverage  area  of  the 
plan,  while  the  contours  underneath  show  the  altitude  normalized  maximum 
standard  deviation  from  the  CRLB.  The  colorbar  on  the  left  side  represents 
the  uncertainty  level. 


uncertainty  does  not  grow  rapidly  until  it  passes  the  camera 
measurement  threshold,  C thresh-  Once  the  cross-trackline- 
space  exceeds  that  of  the  camera  measurement  threshold,  it  no 
longer  makes  cross-track  links,  which  significantly  increases 
the  maximum  trajectory  uncertainty  (e.g.  plan  E).  Therefore, 
without  considering  the  coverage  area,  it  would  be  reasonable 
to  select  the  value  of  d  and  h  according  to  the  camera 
measurement  threshold,  C thresh,  which  is  a  function  of  the 
camera’s  intrinsic  parameters,  field  of  view,  and  scene  altitude. 
However,  since  the  uncertainty  and  coverage  area  should 
be  evaluated  simultaneously,  a  contour  plot  containing  both 
factors  is  plotted  as  in  Fig.  6,  which  can  be  used  to  analyze 
and  optimally  plan  the  survey. 

In  the  contour  plot,  each  sample  path  is  marked  with  capital 
letters  from  A  to  E.  Path  C  has  been  selected  to  be  optimal  in 
that  it  achieves  the  minimum  uncertainty  for  coverage  of  the 


largest  area  among  the  parameters  for  the  same  uncertainty 
level.  With  plan  C  (d  =  0.7  and  h  =  0.5),  an  altitude 
normalized  max  standard  deviation  of  0.017  is  estimated  for 
an  altitude  normalized  total  coverage  area  of  10.4. 

III.  Simulation  Result 

We  examine  our  previous  hull-inspection  survey  results  of 
the  USS  Saratoga  [13]  using  our  newfound  CRLB  analysis, 
and  look  for  possible  improvements  that  we  could  have  made 
to  the  original  survey  trajectory  by  analyzing  the  relationship 
between  the  CRLB  and  survey  design  parameters.  We  continue 
to  use  the  maximum  of  the  covariance  matrix  of  all  nodes 
in  the  graph  as  our  uncertainty  metric  (max^S^..  |),  which 
yields  the  contour  plot  depicted  in  Fig.  7. 

A.  Improvement  in  the  Saratoga  Survey 

The  previous  USS  Saratoga  survey  was  planned  using 
along-trackline-space,  d ,  and  cross-trackline-space,  h,  which 
were  calculated  based  upon  the  desired  percentage  of  image 
overlap  without  taking  into  account  any  effect  on  pose  un¬ 
certainty.  The  vehicle  survey  was  configured  for  a  constant 
distance  from  the  ship  hull,  maintaining  a  standoff  distance  of 
1.5  m  from  the  hull.  To  maintain  a  cross-track  image  overlap 
percentage  of  54%,  h  was  calculated  to  be  0.5  m.  We  did 
not  intentionally  control  the  along-trackline-space  d ,  rather 
we  set  the  camera  to  run  at  a  fixed  frame  rate  of  1  fps, 
which  at  a  forward  speed  of  0.2  m/s  resulted  in  an  along-track 
sample  spacing  of  approximately  0.2  m.  With  d  —  0.2  m  and 
h  =  0.5  m,  a  large  along-track  image  overlap  ratio  of  81.68% 
was  achieved  for  the  overall  survey.  According  to  the  present 
CRLB  analysis,  the  minimum  achievable  uncertainty  for  this 
survey  is  calculated  to  be  0.0141  m  using  1060  nodes  to  cover 
a  89.40  m2  area.  This  previous  survey  plan  is  denoted  with  the 
letter  G  in  Fig.  7. 

To  calculate  the  optimal  survey  parameters,  the  uncertainty 
and  the  coverage  area  are  evaluated  with  respect  to  different 
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(a)  Evaluation  of  the  uncertainty  level  and  the  coverage  area 
with  respect  to  design  parameters 
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Fig.  7.  Parameter  evaluation  for  Saratoga  data.  Point  G  indicates  the  previous 
survey  plan.  We  propose  two  improved  plans  marked  as  H  and  I. 


sets  of  the  design  parameters  as  in  Fig.  7.  Our  goal  is  to  find 
the  set  of  parameters  that  covers  the  largest  area  with  the 
lowest  uncertainty.  Here,  by  examining  the  contour  plot  of 
Fig.  7,  we  can  find  two  sets  of  parameters  that  will  improve 
the  result,  which  are  summarized  in  Table  I  below. 


TABLE  I 

Optimal  USS  Saratoga  Survey  Parameters 


Plan 

d  (m) 

h  (m) 

max  $/\Sxy..\  (m) 

Area  (m2) 

G 

0.2 

0.5 

0.0141 

89.4 

H 

0.45 

0.3 

0.0103 

132.6 

(48.3%) 

I 

0.5 

0.6 

0.0448 

293.5 

(228.3%) 

The  first  plan,  H,  reduces  the  uncertainty  while  simulta¬ 
neously  increasing  the  coverage  area.  This  strategy  moves 
the  original  survey  point  G  in  the  contour  plot  down  toward 
the  lowest  uncertainty  contour,  then  right  toward  the  largest 
possible  coverage  area  for  that  level  of  uncertainty.  This  plan 
provides  26.9%  reduced  uncertainty  while  covering  48.3% 
more  area.  On  the  other  hand,  considering  the  fact  that  the 
survey  is  already  highly  conservative  and  that  the  uncertainty  is 
significantly  small,  we  could  accept  the  increase  of  uncertainty 
for  the  purpose  of  achieving  a  dramatically  larger  survey 
coverage.  This  strategy  is  illustrated  by  point  I  on  the  contour 


15  20 

distance  along  ship  hull  [m] 


Fig.  8.  Original  and  simulated  trajectories  for  the  two  improved  surveys. 


(a)  Re-planned  survey  I  by  node  removal 


(b)  Feature  distribution  information  from  the  original  survey 


Fig.  9.  The  area  without  enough  feature  distribution  is  revealed  from  the 
previous  survey.  The  improved  survey  plan  is  proposed  as  in  (a).  Ellipsoids  at 
each  node  in  (a)  illustrates  1  —  a  contour  of  the  position  uncertainty,  which 
are  inflated  by  10  for  visibility. 


graph  where  the  uncertainty  increases  to  0.0448  m,  (but  is  still 
in  absolute  terms  small)  while  covering  228.3%  more  area. 
The  two  sample  paths  generated  from  these  two  improved 
surveys  are  illustrated  in  Fig.  8. 

B.  Exploiting  Feature  Distribution  in  the  Survey  Design 

The  distribution  of  features  and  texture  in  the  environment 
is  a  highly  dominant  factor  in  the  SLAM  sensor  measurement 
quality.  For  example,  for  the  vision  system  the  distribution  of 
features  in  the  environment  is  critical  to  registration  quality, 
and  it  will  determine  the  performance  of  the  camera  mea¬ 
surement.  However,  real  environments  typically  do  not  exhibit 
uniform  feature  distributions.  There  therefore  exist  areas  in  the 
survey  where  the  measurement  might  not  be  reliable  due  to 
feature-less  regions  (e.g.,  sand  areas,  empty  regions  or  areas 
of  low  texture).  Hence,  when  we  have  information  about  the 
environment  prior  to  our  survey,  or  when  we  re-plan  the  same 
area  with  data  from  a  previous  survey,  we  can  exploit  this 
knowledge  in  our  survey  design  in  order  to  cover  more  area 
by  reducing  our  pose  sampling  in  any  featureless  areas.  In  the 


case  of  the  ship  hull  survey,  many  featureless  region  exists 
and  fail  to  provide  relative-pose  constraints.  By  removing 
nodes  from  those  areas  that  are  expected  to  be  inefficient,  we 
could  use  the  saved  energy  from  the  removal  of  the  nodes  to 
conduct  further  exploration,  despite  the  expense  of  temporarily 
increased  uncertainty  over  that  region. 

The  simulated  result  of  the  removal  of  nodes  with  the  same 
design  parameters  of  plan  I  (Fig.  8)  is  provided  in  Fig.  9.  From 
the  result  of  the  previous  survey  in  Fig.  9(b),  we  know  that 
the  area  of  10  m  by  2  m  is  a  largely  feature-less  region,  which 
is  equivalent  to  66  nodes  in  the  graph.  Therefore,  as  can  be 
seen  Fig.  9(a),  these  66  nodes  were  removed  from  the  middle 
section,  extending  the  total  path  length  but  maintaining  an 
uncertainty  level  equivalent  to  that  in  plan  I. 

IV.  Conclusion 

This  paper  described  a  method  for  improving  the  localiza¬ 
tion  performance  of  a  pose-graph  SLAM  methodology  using 
sequential/non-sequential  relative-pose  constraints.  A  conser¬ 
vative  static  uncertainty  bound  was  examined  by  calculating 
the  CRLB  in  the  AUV  survey  design  phase,  and  thereby 
determining  the  uncertainty  level  of  the  graph.  A  graphical 
design  tool  based  upon  contour  plots  of  coverage  area  and 
maximum  uncertainty  was  developed  to  interpret  the  effects  of 
the  design  parameters  on  the  survey  coverage  and  uncertainty. 
Using  this  tool,  design  parameters  were  proposed  to  meet  the 
purpose  of  the  survey  as  a  pre-planning  instrument.  A  method 
to  exploit  the  environmental  information  for  use  in  the  survey 
was  also  proposed. 

In  this  preliminary  result,  we  assumed  a  fixed  number  of 
nodes  in  the  graph  and  found  the  two  design  parameters  for 
a  simple  boustrophedon  trajectory.  Future  work  will  include 
modeling  additional  terms  important  to  the  survey  design,  such 
as  the  total  amount  of  time  of  the  mission  or  the  total  energy 
limitation  of  the  vehicle.  The  total  number  of  nodes  in  the 
graph  should  be  variable  as  a  function  of  the  amount  of  energy 
the  vehicle  can  expend  and  the  overall  trajectory  path  length. 
Similar  contour  plots  could  be  generated  with  this  information 
on  it,  and  we  could  use  the  same  graphical  method  to  optimize 
the  survey  in  the  pre-planning  phase. 

Acknowledgments 

This  work  is  supported  in  part  through  a  grant  from  the 
Office  of  Naval  Research  (ONR)  (Award  #N000 14-07- 1-0791) 
and  the  National  Science  Foundation  (NSF)  (Award  #IIS 
0746455). 

References 

[1]  H.  C.  Brown,  A.  Kim,  and  R.  M.  Eustice,  “An  overview  of  autonomous 
underwater  vehicle  research  and  testbed  at  PeRL,”  Marine  Technology 
Society  Journal ,  vol.  43,  no.  2,  pp.  33-47,  2009. 

[2]  J.  Vaganay,  M.  Elkins,  S.  Willcox,  F.  Hover,  R.  Damus,  S.  Desset, 
J.  Morash,  and  V.  Polidoro,  “Ship  hull  inspection  by  hull-relative 
navigation  and  control,”  OCEANS,  2005.  Proceedings  of  MTS/IEEE,  pp. 
761-766  Vol.  1,  2005. 

[3]  N.  Fairfield,  G.  A.  Kantor,  D.  Jonak,  and  D.  Wettergreen,  “DEPTHX  au¬ 
tonomy  software:  design  and  field  results,”  Robotics  Institute,  Carnegie 
Mellon  University,  Pittsburgh,  PA,  Tech.  Rep.  CMU-RI-TR-08-09,  July 
2008. 


[4]  R.  M.  Eustice,  H.  Singh,  J.  J.  Leonard,  and  M.  R.  Walter,  “Visually 
mapping  the  RMS  Titanic:  conservative  covariance  estimates  for  SLAM 
information  filters,”  International  Journal  of  Robotics  Research ,  vol.  25, 
no.  12,  pp.  1223-1242,  2006. 

[5]  H.  Singh,  J.  Howland,  and  O.  Pizarro,  “Advances  in  large-area  photo- 
mosaicking  underwater,”  IEEE  Journal  of  Oceanic  Engineering,  vol.  29, 
no.  3,  pp.  872-886,  Jul.  2004. 

[6]  J.  Farrell,  S.  Pang,  and  W.  Li,  “Chemical  plume  tracing  via  an  au¬ 
tonomous  underwater  vehicle,”  Oceanic  Engineering,  IEEE  Journal  of 
vol.  30,  no.  2,  pp.  428-442,  April  2005. 

[7]  R.  Armstrong,  H.  Singh,  J.  Torres,  R.  Nemeth,  A.  Can,  C.  Roman, 
R.  Eustice,  L.  Riggs,  and  G.  Garcia-Moliner,  “Characterizing  the  deep 
insular  shelf  coral  reef  habitat  of  the  Hind  Bank  marine  conservation 
district  (US  Virgin  Islands)  using  the  Seabed  autonomous  underwater 
vehicle,”  Continental  Shelf  Research,  vol.  26,  no.  2,  pp.  194-205,  Feb. 
2006. 

[8]  D.  Yoerger,  A.  Bradley,  H.  Singh,  B.  Walden,  M.  Cormier,  and  W.  Ryan, 
“Multisensor  mapping  of  the  deep  seafloor  with  the  Autonomous  Benthic 
Explorer,”  in  Proceedings  of  the  International  Symposium  on  Unmanned 
Untethered  Submersible  Technology,  Tokyo,  Japan,  May  2000,  pp.  248- 
253. 

[9]  O.  Pizarro,  R.  Eustice,  and  H.  Singh,  “Large  area  3D  reconstructions 
from  underwater  surveys,”  in  Proceedings  of  the  IEEE/MTS  OCEANS 
Conference  and  Exhibition,  vol.  2,  Kobe,  Japan,  Nov.  2004,  pp.  678-687. 

[10]  Y.  Petillot,  I.  Tena  Ruiz,  and  D.  Lane,  “Underwater  vehicle  obstacle 
avoidance  and  path  planning  using  a  multi-beam  forward  looking  sonar,” 
IEEE  Journal  of  Oceanic  Engineering,  vol.  26,  no.  2,  pp.  240-251,  2001. 

[11]  D.  Kruger,  R.  Stolkin,  A.  Blum,  and  J.  Briganti,  “Optimal  AUV 
path  planning  for  extended  missions  in  complex,  fast-flowing  estuarine 
environments,”  in  ICRA,  2007,  pp.  4265-4270. 

[12]  K.  Carroll,  S.  McClaran,  E.  Nelson,  D.  Barnett,  D.  Friesen,  and 
G.  William,  “Auv  path  planning:  an  a*  approach  to  path  planning  with 
consideration  of  variable  vehicle  speeds  and  multiple,  overlapping,  time- 
dependent  exclusion  zones,”  Autonomous  Underwater  Vehicle  Technol¬ 
ogy,  1992.  AUV  ’92.,  Proceedings  of  the  1992  Symposium  on,  pp.  79-84, 
Jun  1992. 

[13]  A.  Kim  and  R.  M.  Eustice,  “Pose-graph  visual  SLAM  with  geometric 
model  selection  for  autonomous  underwater  ship  hull  inspection,”  in 
Proceedings  of  the  IEEE/RSJ  International  Conference  on  Intelligent 
Robots  and  Systems,  2009,  To  Appear. 

[14]  C.  Roman  and  H.  Singh,  “Improved  vehicle  based  multibeam  bathymetry 
using  sub-maps  and  SLAM,”  in  Proceedings  of  the  IEEE/RSJ  Interna¬ 
tional  Conference  on  Intelligent  Robots  and  Systems,  Edmonton,  Alberta, 
Canada,  Aug.  2005,  pp.  2422-2429. 

[15]  E.  Olson,  “Real-time  correlative  scan  matching,”  in  Proceedings  of  the 
IEEE  International  Conference  on  Robotics  and  Automation  (ICRA), 
Kobe,  Japan,  June  2009,  pp.  4387-4393. 

[16]  Y.  Bar-Shalom,  X.  Rong  Li,  and  T.  Kirubarajan,  Estimation  with 
applications  to  tracking  and  navigation.  New  York:  John  Wiley  & 
Sons,  Inc.,  2001. 

[17]  M.  Bryson  and  S.  Sukkarieh,  “An  information-theoretic  approach  to 
autonomous  navigation  and  guidance  of  an  uninhabited  aerial  vehicle 
in  unknown  environments,”  in  Intelligent  Robots  and  Systems,  2005. 
(IROS  2005).  2005  IEEE/RSJ  International  Conference  on,  Aug.  2005, 
pp.  3770-3775. 

[18]  B.  Bingham,  “Precision  autonomous  underwater  navigation,”  Ph.D. 
dissertation,  Massachusetts  Institute  of  Technology,  May  2003. 

[19]  K.  Dogangay  and  H.  Hmam,  “Optimal  angular  sensor  separation  for 
AOA  localization,”  Signal  Process.,  vol.  88,  no.  5,  pp.  1248-1260,  2008. 

[20]  R.  Sim  and  N.  Roy,  “Global  a-optimal  robot  exploration  in  SLAM,” 
in  Proceedings  of  the  IEEE  International  Conference  on  Robotics  and 
Automation  (ICRA),  Barcelona,  Spain,  2005,  pp.  661-666. 

[21]  T.  Vidal-Calleja,  A.  Davison,  J.  Andrade-Cetto,  and  D.  Murray,  “Active 
control  for  single  camera  SLAM,”  in  IEEE  Int  Conf  on  Robotics  and 
Automation,  Orlando,  May  2006,  2006,  pp.  1930-1936. 

[22]  Teledyne  RD  Instruments,  “Explorer  DVL,”  World  Wide 
Web  electronic  publication,  2008.  [Online].  Available: 
http://www.rdinstruments.com/explorer.html 

[23]  R.  M.  Eustice,  O.  Pizarro,  and  H.  Singh,  “Visually  augmented  navi¬ 
gation  for  autonomous  underwater  vehicles,”  IEEE  Journal  of  Oceanic 
Engineering,  vol.  33,  no.  2,  pp.  103-122,  Apr.  2008. 


